/*
 * Inertial.c
 *
 *  Created on: 2012.12.28.
 *      Author: Tibi
 */
#include "L3GD20.h"
#include "stm32f4_discovery_lis302dl.h"
#include "Inertial.h"

Inertial inertia;
Offsets offset = {0,0,0,0,0,0};

void Init_Inertial(void)
{
	LIS302DL_InitTypeDef accinit;
	accinit.Power_Mode = LIS302DL_LOWPOWERMODE_ACTIVE;
	accinit.Axes_Enable = 0;
	accinit.Full_Scale = LIS302DL_SENSITIVITY_2_3G;
	accinit.Output_DataRate = LIS302DL_DATARATE_100;
	accinit.Self_Test = LIS302DL_SELFTEST_NORMAL;
	LIS302DL_Init(&accinit);
	L3GD20_Init(0x1F);
	inertia.rotx = 0;
	inertia.roty = 0;
	inertia.rotz = 0;
}

void Inertial_Get(void)
{
	int8_t acc[6];
	int16_t gyro[3];
	uint8_t i;

	LIS302DL_Read((uint8_t*)acc, LIS302DL_OUT_X_ADDR, 6);
	inertia.accx = acc[0] * 0.018 - offset.accx;
	inertia.accy = acc[2] * 0.018 - offset.accy;
	inertia.accz = acc[4] * 0.018 - offset.accz;

	for(i = 0; i < 6; i++)
		L3GD20_Read((uint8_t*)gyro+i, L3GD20_OUT_X_ADDR+i, 1);

	inertia.gyrox = gyro[0] * 0.00875 - offset.gyrox;
	inertia.gyroy = gyro[1] * 0.00875 - offset.gyroy;
	inertia.gyroz = gyro[2] * 0.00875 - offset.gyroz;

	inertia.rotx += inertia.gyrox;
	inertia.roty += inertia.gyroy;
	inertia.rotz += inertia.gyroz;
}
